function [out] = sinistepwrapper_pgs(mode, in, par, iniCon) %#codegen
%NAVINT 惯性导航解算周期
%
% Input Arguments:
% mode: int8标量，运行模式: 0为初始化，1为正常的导航解算周期
% in.INS.accOut: 3*1矩阵，输入为加速度计输出脉冲数，单位^，输出为经卡尔曼滤波器补偿后的比速度增量，单位m/s/dT
% in.INS.gyroOut: 3*1矩阵，输入为陀螺输出脉冲数，单位^，输出为经卡尔曼滤波器补偿后的角增量，单位rad/dT
% in.g: 标量，外部给出的重力加速度，单位m/s^2
% par.dTINS: 标量，惯导解算周期，单位s
% par.IMU.acc: 结构体，各元素为加速度计各项参数，单位均为国际单位制单位
% par.IMU.gyro: 结构体，各元素为陀螺各项参数，单位均为国际单位制单位
% iniCon.pos: 3*1向量，初始位置，各元素分别为纬度、经度、高度，单位分别为rad、rad、m
% iniCon.vG: 3*1向量，初始速度，各元素分别为东向，北向，天向速度，单位为m/s
% iniCon.CBN: 3*3矩阵，初始姿态
%
% Output Arguments:
% out.angleVel: 3*1向量，角速率，单位为rad/s
% out.acc: 3*1向量，加速度，单位为m/s^2
% out.pos: 3*1向量，导航位置，各列分别为纬度、经度、高度，单位分别为rad、rad、m
% out.vG: 3*1向量，G系下的导航速度，各列分别为东、北、天向速度，单位为m/s
% out.att: 3*1向量，导航姿态，各列分别为滚动、俯仰、偏航，（为由地理坐标系为NUE，分别绕Y、Z、X轴转动负航向角、俯仰角、滚转角得到B系）单位为rad
%

coder.inline('never');

persistent p_par_navInt p_iniCon_navInt p_out_navInt

cst_CLN = [0 1 0; 1 0 0; 0 0 -1];
cst_CNN1 = [0 1 0; 0 0 1; 1 0 0]; % 由ENU转到NUE

if isempty(p_par_navInt)
    p_par_navInt.dTINS = coder.nullcopy(NaN);
    p_par_navInt.g = coder.nullcopy(NaN);
    p_par_navInt.IMU.acc.SF0 = coder.nullcopy(NaN(3, 1));
    p_par_navInt.IMU.acc.B = coder.nullcopy(NaN(3, 1));
    p_par_navInt.IMU.acc.dSF = coder.nullcopy(NaN(3, 1));
    p_par_navInt.IMU.acc.MA = coder.nullcopy(NaN(3, 3));
    p_par_navInt.IMU.acc.SO = coder.nullcopy(NaN(3, 1));
    p_par_navInt.IMU.gyro.SF0 = coder.nullcopy(NaN(3, 1));
    p_par_navInt.IMU.gyro.B = coder.nullcopy(NaN(3, 1));
    p_par_navInt.IMU.gyro.dSF = coder.nullcopy(NaN(3, 1));
    p_par_navInt.IMU.gyro.MA = coder.nullcopy(NaN(3, 3));
    p_par_navInt.IMU.gyro.GS = coder.nullcopy(NaN(3, 3));
end
if isempty(p_iniCon_navInt)
    p_iniCon_navInt.pos = coder.nullcopy(NaN(3, 1));
    p_iniCon_navInt.vG = coder.nullcopy(NaN(3, 1));
    p_iniCon_navInt.CBN = coder.nullcopy(NaN(3, 3));
end
if isempty(p_out_navInt)
    p_out_navInt.angleVel = coder.nullcopy(NaN(3, 1));
    p_out_navInt.acc = coder.nullcopy(NaN(3, 1));
    p_out_navInt.pos = coder.nullcopy(NaN(3, 1));
    p_out_navInt.vG = coder.nullcopy(NaN(3, 1));
    p_out_navInt.att = coder.nullcopy(NaN(3, 1));
end

switch mode
    case int8(0)
        
        %% 设置参数值
        p_par_navInt.dTINS = par.dTINS;
        p_par_navInt.g = in.g;
        p_par_navInt.IMU.acc.SF0 = par.IMU.acc.SF0;
        p_par_navInt.IMU.acc.B = par.IMU.acc.B;
        p_par_navInt.IMU.acc.dSF = par.IMU.acc.dSF;
        p_par_navInt.IMU.acc.MA = par.IMU.acc.MA;
        p_par_navInt.IMU.acc.SO = par.IMU.acc.SO;
        p_par_navInt.IMU.gyro.SF0 = par.IMU.gyro.SF0;
        p_par_navInt.IMU.gyro.B = par.IMU.gyro.B;
        p_par_navInt.IMU.gyro.dSF = par.IMU.gyro.dSF;
        p_par_navInt.IMU.gyro.MA = par.IMU.gyro.MA;
        p_par_navInt.IMU.gyro.GS = par.IMU.gyro.GS;
        
        p_iniCon_navInt.pos = iniCon.pos;
        p_iniCon_navInt.vG = iniCon.vG;
        p_iniCon_navInt.CBL = cst_CLN' * iniCon.CBN;
        
        %% 初始化
        % 标定补偿
        in.INS.accOut = sensorout2in_newton(in.INS.accOut, p_par_navInt.IMU.acc.SF0, p_par_navInt.IMU.acc.B*p_par_navInt.dTINS, ...
            diag(p_par_navInt.IMU.acc.dSF)+p_par_navInt.IMU.acc.MA, p_par_navInt.IMU.acc.SO/p_par_navInt.dTINS);
        in.INS.gyroOut = sensorout2in_linear(in.INS.gyroOut, p_par_navInt.IMU.gyro.SF0, p_par_navInt.IMU.gyro.B*p_par_navInt.dTINS, ...
            diag(p_par_navInt.IMU.gyro.dSF)+p_par_navInt.IMU.gyro.MA, in.INS.accOut, p_par_navInt.IMU.gyro.GS);
        
        % 常数
        cst_SINI = load('earthconstants_wgs84.mat', 'RE', 'f', 'mu', 'J2', 'J3', 'omegaIE');
        % 输入
        in_SINI = struct('CNEExt', NaN(3), 'hExt', NaN, 'vNExt', NaN(3, 1), 'CBLExt', NaN(3), 'qBLExt', NaN(1, 4), 'hAltm', 0, ...
            'specVelInc', NaN(3, 1), 'angleInc', NaN(3, 1), 'g', in.g);
        % 配置
        cfg_SINI.NSInLS = 1;
        cfg_SINI.HSInNS = 1;
        cfg_SINI.useDCMInAttCalc = true;
        cfg_SINI.useNormOrthoInAttCalc = true;
        cfg_SINI.useHiResPosNSCal = true;
        cfg_SINI.useExtGravity = false;
        cfg_SINI.navCoordType = NavCoordType.GEO;
        % 参量
        par_SINI.Tl = par.dTINS;
        par_SINI.C = [0 0 0];
        % 初始条件
        iniCon_SINI.L_n = p_iniCon_navInt.pos(1, 1);
        iniCon_SINI.L_n1 = p_iniCon_navInt.pos(1, 1);
        iniCon_SINI.lambda_n = p_iniCon_navInt.pos(2, 1);
        iniCon_SINI.lambda_n1 = p_iniCon_navInt.pos(2, 1);
        iniCon_SINI.h_n = p_iniCon_navInt.pos(3, 1);
        iniCon_SINI.h_n1 = p_iniCon_navInt.pos(3, 1);
        iniCon_SINI.vN_m = p_iniCon_navInt.vG;
        iniCon_SINI.vN_m1 = p_iniCon_navInt.vG;
        iniCon_SINI.vN_n1 = p_iniCon_navInt.vG;
        iniCon_SINI.CBL = p_iniCon_navInt.CBL;
        iniCon_SINI.specVelInc_lx = in.INS.accOut;
        iniCon_SINI.angleInc_lx = in.INS.gyroOut;
        sinistep_pgs(0, in_SINI, cfg_SINI, par_SINI, iniCon_SINI, cst_SINI);
        
    case int8(1)
        %% 导航解算
        % 标定补偿
        in.INS.accOut = sensorout2in_newton(in.INS.accOut, p_par_navInt.IMU.acc.SF0, p_par_navInt.IMU.acc.B*p_par_navInt.dTINS, ...
            diag(p_par_navInt.IMU.acc.dSF)+p_par_navInt.IMU.acc.MA, p_par_navInt.IMU.acc.SO/p_par_navInt.dTINS);
        in.INS.gyroOut = sensorout2in_linear(in.INS.gyroOut, p_par_navInt.IMU.gyro.SF0, p_par_navInt.IMU.gyro.B*p_par_navInt.dTINS, ...
            diag(p_par_navInt.IMU.gyro.dSF)+p_par_navInt.IMU.gyro.MA, in.INS.accOut, p_par_navInt.IMU.gyro.GS);
        in_SINI = struct('CNEExt', NaN(3), 'hExt', NaN, 'vNExt', NaN(3, 1), 'CBLExt', NaN(3), 'qBLExt', NaN(1, 4), 'hAltm', 0, ...
            'specVelInc', in.INS.accOut, 'angleInc', in.INS.gyroOut, 'g', in.g);
        
        [out_SINI] = sinistep_pgs(1, in_SINI);
        p_out_navInt.angleVel = in.INS.gyroOut/p_par_navInt.dTINS;
        p_out_navInt.acc = in.INS.accOut/p_par_navInt.dTINS;
        p_out_navInt.pos = [out_SINI.L, out_SINI.lambda, out_SINI.h]';
        p_out_navInt.vG = out_SINI.vG;
        [psi, theta, phi] = dcm2angle_cg((cst_CNN1*out_SINI.CBG)', 'YZX');
        psi = wrap(-psi, 0, 2*pi);
        p_out_navInt.att = [phi theta psi]';
end
out = p_out_navInt;
end